/******************************************************************************
 * The MIT License
 *
 * Copyright (c) 2010, LeafLabs, LLC.
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use, copy,
 * modify, merge, publish, distribute, sublicense, and/or sell copies
 * of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *****************************************************************************/

#include "Servo.h"
/**
 * @brief   伺服舵机绑定PWM IO
 * @param  pObj             舵机对象
 * @param  pIO              PWM IO对象
 * @return int32_t          绑定结果
 */
int32_t SERVO_AttachIO(SERVO_Object_t *pObj, SERVO_IO_t *pIO)
{
  int32_t ret = ATTACHED;
  if(pObj == NULL || pIO == NULL)
  {
    ret = NOT_ATTACHED;
  }
  else
  {
    pObj->IO.Init               = pIO->Init;
    pObj->IO.WritePulseWidth    = pIO->WritePulseWidth;

    if(pObj->IO.Init == NULL)
    {
      ret = NOT_ATTACHED;
    }else
    {
      pObj->IO.Init();
    }
  }
  return ret;
}
/**
 * @brief   伺服舵机初始化
 * @param  pObj             舵机对象
 * @param  initAngle        初始化角度
 * @param  minAngle         活动范围最小角度
 * @param  maxAngle         活动范围最大角度
 * @return int32_t 
 */
int32_t SERVO_Init(SERVO_Object_t *pObj, uint8_t initAngle, uint8_t minAngle, uint8_t maxAngle)
{
  int32_t ret = INIT_SUCCESS;
  if(pObj->IO.Init == NULL)
  {
    ret = INIT_ERROR;
  }
  else
  {
    pObj->IO.Init();
    if((minAngle > maxAngle) || maxAngle > MAX_ANGLE)
    {
      ret = INIT_ERROR;
    }
    else if(initAngle >= maxAngle)
    {
      ret = INIT_ERROR;
    }
    else
    {
      pObj->minAngle = minAngle;
      pObj->maxAngle = maxAngle;
      pObj->zeroAngle = initAngle;
    }
    SERVO_WriteAngle(pObj, pObj->zeroAngle);
  }
  return ret;
}
/**
 * @brief   伺服舵机设置初始角度
 * @param  pObj             舵机对象
 * @param  zero             角度值 0 - 180
 */
void SERVO_SetZero(SERVO_Object_t *pObj, uint8_t zero)
{
  pObj->zeroAngle = zero;
}
/**
 * @brief   伺服舵机设置角度
 * @param  pObj             舵机对象
 * @param  target_angle     角度值 0 - 180
 */
void SERVO_WriteAngle(SERVO_Object_t *pObj, uint8_t target_angle)
{
  uint16_t real_pulse_width;
  if(target_angle > 180) return;
  real_pulse_width = target_angle * (MAX_PULSE_WIDTH - MIN_PULSE_WIDTH) / 180 + MIN_PULSE_WIDTH;
  pObj->IO.WritePulseWidth(real_pulse_width);
}
/**
 * @brief   伺服舵机设置相对角度
 * @param  pObj             舵机对象
 * @param  target_angle     相对初始角度的角度 -180 - 180
 */
void SERVO_ChangeAngle(SERVO_Object_t *pObj, int16_t target_angle)
{
  int16_t real_angle;
  real_angle = (int16_t)pObj->zeroAngle + target_angle;

  if(real_angle > (int16_t)(pObj->maxAngle))  real_angle = (int16_t)(pObj->maxAngle);
  if(real_angle < (int16_t)(pObj->minAngle))  real_angle = (int16_t)(pObj->minAngle);

  SERVO_WriteAngle(pObj, (uint8_t)real_angle);
}